#pragma once
/**
 ********************************************************************************************************
 *                                               示例代码
 *                                             EXAMPLE  CODE
 *
 *                      (c) Copyright 2024; SaiShu.Lcc.; Leo; https://bjsstech.com
 *                                   版权所属[SASU-北京赛曙科技有限公司]
 *
 *            The code is for internal use only, not for commercial transactions(开源学习,请勿商用).
 *            The code ADAPTS the corresponding hardware circuit board(代码适配百度Edgeboard-智能汽车赛事版),
 *            The specific details consult the professional(欢迎联系我们,代码持续更正，敬请关注相关开源渠道).
 *********************************************************************************************************
 * @file ring.cpp
 * @author Leo
 * @brief 环岛识别（基于track赛道识别后）
 * @version 0.1
 * @date 2022-02-28
 *
 * @copyright Copyright (c) 2022
 *
 * @note  环岛识别步骤（ringStep）：
 *          1：环岛识别（初始化）
 *          2：入环处理
 *          3：环中处理
 *          4：出环处理
 *          5：出环结束
 */

#include <fstream>
#include <iostream>
#include <cmath>
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include "../../include/common.hpp"
#include "tracking.cpp"

using namespace cv;
using namespace std;

class Ring
{
public:
    uint16_t counterShield = 0; // 环岛检测屏蔽计数器：屏蔽车库误检测

    /**
     * @brief 环岛识别初始化|复位
     *
     */
    void reset(void)
    {
        RingType ringType = RingType::RingLeft; // 环岛类型
        RingStep ringStep = RingStep::None;     // 环岛处理阶段
        int rowRepairLine = 0;                  // 用于环补线的点（行号）
        int colRepairLine = 0;                  // 用于环补线的点（列号）
        counterSpurroad = 0;                    // 岔路计数器
        counterShield = 0;
    }
    /**
     * @brief 环岛识别与行径规划
     *
     * @param track 基础赛道识别结果
     * @param imagePath 赛道路径图像
     */
    bool process(Tracking &track, Mat &imagePath)
    {
        if (counterShield < 40)
        {
            counterShield++;
            return false;
        }

        bool ringEnable = false;                                 // 判环标志
        RingType ringTypeTemp = RingType::RingNone;              // 环岛类型：临时变量
        int rowBreakpointLeft = 0;                               // 边缘拐点起始行（左）
        int rowBreakpointRight = 0;                              // 边缘拐点起始行（右）
        int colEnterRing = 0;                                    // 入环点（图像列序号）
        int rowRepairRingside = track.widthBlock.size() - 1;     // 环一侧，补线起点（行号）
        int rowRepairStraightside = track.widthBlock.size() - 1; // 直道侧，补线起点（行号）
        int rowYendStraightside = track.widthBlock.size() - 1;   // 直道侧，延长补线终点（行号）
        _index = 0;
        _ringPoint = POINT(0, 0);

        // 算环用布线的候选点
        rowRepairLine = max(rowRepairLine - 5, 0);
        if (ringStep == RingStep::Entering && !track.spurroad.empty())
        {
            if (ringType == RingType::RingLeft && track.checkPointsEdgeLeft.size() > 20)
            {
                for (int j = max(rowRepairLine - 30, 10);
                     j < track.checkPointsEdgeLeft.size() - 10 && j < rowRepairLine + 30 &&
                     track.checkPointsEdgeLeft[j].x >= track.spurroad[0].x;
                     j++)
                {
                    if (track.checkPointsEdgeLeft[j].y > track.checkPointsEdgeLeft[j - 10].y &&
                        track.checkPointsEdgeLeft[j].y > track.checkPointsEdgeLeft[j + 10].y)
                    {
                        rowRepairLine = j;
                        break;
                    }
                }
            }
            else if (ringType == RingType::RingRight && track.checkPointsEdgeRight.size() > 20)
            {
                for (int j = max(rowRepairLine - 30, 10);
                     j < track.checkPointsEdgeRight.size() - 10 && j < rowRepairLine + 30 &&
                     track.checkPointsEdgeRight[j].x >= track.spurroad[0].x;
                     j++)
                {
                    if (track.checkPointsEdgeRight[j].y < track.checkPointsEdgeRight[j - 10].y &&
                        track.checkPointsEdgeRight[j].y < track.checkPointsEdgeRight[j + 10].y)
                    {
                        rowRepairLine = j;
                        break;
                    }
                }
            }
        }

        // 搜索赛道左右边缘满足图像边沿的最高处
        for (int ii = 0; ii < track.checkPointsEdgeLeft.size(); ++ii)
        {
            rowBreakpointLeft = track.checkPointsEdgeLeft[ii].x;
            if (track.checkPointsEdgeLeft[ii].y > 2)
                break;
        }
        for (int ii = 0; ii < track.checkPointsEdgeRight.size(); ++ii)
        {
            rowBreakpointRight = track.checkPointsEdgeRight[ii].x;
            if (track.checkPointsEdgeRight[ii].y < COLSIMAGE - 3)
                break;
        }

        // 判环
        int countWide = 0; // 环岛入口变宽区域行数
        for (int i = 1; i < track.widthBlock.size(); ++i)
        {
            // cout << "判环" << track.widthBlock[i].y << " "
            //      << track.widthBlock[i - 1].y << " " << track.widthBlock[i].y << " " << track.widthBlock[i].x << endl;
            if (track.widthBlock[i].y > track.widthBlock[i - 1].y && track.widthBlock[i].y > COLSIMAGE * 0.65 && track.widthBlock[i].x > 30 &&
                ((track.stdevLeft > 120 && track.stdevRight < 50) || (track.stdevLeft < 50 && track.stdevRight > 120) || ringStep == RingStep::Entering)) // 搜索突然变宽的路径行数
            {
                ++countWide;
                cout << countWide << endl;
            }
            else
            {
                countWide = 0;
            }
            // [1] 入环判断
            if (((ringStep == RingStep::None || ringStep == RingStep::Entering) && countWide >= 5 && !track.spurroad.empty()))
            {
                if (ringTypeTemp == RingType::RingNone) // 环岛方向判定
                {
                    int tmp_flag = 0;
                    for (int j = 0; j < track.spurroad.size(); j++)
                    {
                        if (track.spurroad[j].x < track.checkPointsEdgeLeft[i - 5].x)
                        {
                            tmp_flag = 1;
                        }
                    }
                    if (tmp_flag == 0)
                    {
                        countWide = 0;
                        continue;
                    }
                    bool isRing = true;
                    int countEdge = 0;
                    // 当检测到左边有弧度的时候
                    if (track.checkPointsEdgeLeft[i].y < track.checkPointsEdgeLeft[i - 5].y && track.checkPointsEdgeLeft[i].y >3)
                    {
                        // 判断右边是否连续来区分十字路口和环岛
                        for (int Ri = 0; Ri < track.checkPointsEdgeRight.size() - 1; Ri++)
                        {
                            if (abs(track.checkPointsEdgeRight[Ri].y - track.checkPointsEdgeRight[Ri + 1].y) > 5)
                            {
                                isRing = false;
                                break;
                            }
                            // 记录边缘看不到的数量
                            if (track.checkPointsEdgeRight[Ri].y > 318)
                            {
                                countEdge++;
                            }
                            // 有一半都看不到不能被定为环岛
                            if (countEdge > track.checkPointsEdgeRight.size() / 2)
                            {
                                isRing = false;
                                break;
                            }
                        }
                        if (isRing)
                        {
                            ringTypeTemp = RingType::RingLeft;                 // 环岛类型：左入环
                            colEnterRing = track.checkPointsEdgeLeft[i - 5].y; // 入环点列号
                            _ringPoint.x = track.checkPointsEdgeLeft[i - 5].x;
                            _ringPoint.y = track.checkPointsEdgeLeft[i - 5].y;
                            rowRepairLine = i;                              // 用于环补线的行号
                            colRepairLine = track.checkPointsEdgeLeft[i].x; // 用于环补线的列号
                        }
                    }
                    //改5为10
                    else if (track.checkPointsEdgeRight[i].y > track.checkPointsEdgeRight[i - 5].y && track.checkPointsEdgeRight[i].y < 318)
                    {
                        // 判断左边是否连续来区分十字路口和环岛
                        isRing = true;
                        countEdge = 0;
                        for (int Li = 0; Li < track.checkPointsEdgeLeft.size() - 1; Li++)
                        { // 判断右边是否连续来区分十字路口和环岛,同时不能看不到右边
                            if (abs(track.checkPointsEdgeLeft[Li].y - track.checkPointsEdgeLeft[Li + 1].y) > 5)
                            {
                                isRing = false;
                                break;
                            }
                            // 记录边缘看不到的数量
                            if (track.checkPointsEdgeLeft[Li].y < 2)
                            {
                                countEdge++;
                            }
                            // 有一半都看不到不能被定为环岛
                            if (countEdge > track.checkPointsEdgeLeft.size() / 2)
                            {
                                isRing = false;
                                break;
                            }
                        }
                        if (isRing)
                        {
                            ringTypeTemp = RingType::RingRight;                 // 环岛类型：右入环
                            colEnterRing = track.checkPointsEdgeRight[i - 5].y; // 入环点列号
                            rowRepairLine = i;                                  // 用于环补线的行号
                            colRepairLine = track.checkPointsEdgeRight[i].x;    // 用于环补线的列号
                        }
                    }
                }
                cout << "环岛类型" << ringTypeTemp << endl;
                // 内圆检测
                if ((ringTypeTemp == RingType::RingLeft && colEnterRing - track.checkPointsEdgeLeft[i].y >= 3) ||
                    (ringTypeTemp == RingType::RingRight && track.checkPointsEdgeRight[i].y - colEnterRing >= 3))
                {
                    ringEnable = true;
                    ringStep = RingStep::Entering;
                    ringType = ringTypeTemp;
                    if (rowRepairStraightside == track.widthBlock.size() - 1)
                    {
                        rowRepairStraightside = i - countWide;
                    }
                }
                else
                {
                    countWide = 0;
                }
            }
            /*if(ringStep == RingStep::Entering && ringEnable == false){
                ringEnable = true;
                rowRepairStraightside = rowRepairLine;
            }*/

            if (ringEnable == true && ringStep == RingStep::Entering)
            {
                // cout << "环岛使能" << endl;
                if (ringTypeTemp == RingType::RingLeft)
                {
                    if (track.checkPointsEdgeLeft[i].y <= 2 && i != track.widthBlock.size() - 1)
                    {
                        if (rowRepairRingside == track.widthBlock.size() - 1)
                        {
                            rowRepairRingside = i;
                        }
                        rowYendStraightside = track.checkPointsEdgeLeft[i].x;
                    }
                    else if (rowRepairRingside != track.widthBlock.size() - 1)
                    {

                        int x = track.checkPointsEdgeLeft[rowRepairStraightside].x +
                                (rowYendStraightside -
                                 track.checkPointsEdgeRight[rowRepairStraightside].x) *
                                    5 / 4;
                        int y = (track.checkPointsEdgeLeft[rowRepairStraightside].y +
                                 track.checkPointsEdgeRight[rowRepairStraightside].y) /
                                2;

                        POINT startPoint = track.checkPointsEdgeRight[rowRepairStraightside]; // 补线：起点
                        POINT midPoint(x, y);                                                 // 补线：中点
                        POINT endPoint(rowYendStraightside, 0);                               // 补线：终点

                        vector<POINT> input = {startPoint, midPoint, endPoint};
                        vector<POINT> b_modify = Bezier(0.01, input);
                        track.checkPointsEdgeLeft.resize(rowRepairRingside);
                        track.checkPointsEdgeRight.resize(rowRepairStraightside);
                        for (int kk = 0; kk < b_modify.size(); ++kk)
                        {
                            track.checkPointsEdgeRight.emplace_back(b_modify[kk]);
                        }
                        break;
                    }
                }
                if (ringTypeTemp == RingType::RingRight)
                {
                    if (track.checkPointsEdgeRight[i].y >= 318 && i != track.widthBlock.size() - 1)
                    {
                        if (rowRepairRingside == track.widthBlock.size() - 1)
                        {
                            rowRepairRingside = i;
                        }
                        rowYendStraightside = track.checkPointsEdgeRight[i].x;
                    }
                    else if (rowRepairRingside != track.widthBlock.size() - 1)
                    {

                        int x = track.checkPointsEdgeRight[rowRepairStraightside].x +
                                (rowYendStraightside -
                                 track.checkPointsEdgeLeft[rowRepairStraightside].x) *
                                    5 / 4;
                        int y = (track.checkPointsEdgeRight[rowRepairStraightside].y +
                                 track.checkPointsEdgeLeft[rowRepairStraightside].y) /
                                2;

                        POINT startPoint = track.checkPointsEdgeLeft[rowRepairStraightside]; // 补线：起点
                        POINT midPoint(x, y);                                                // 补线：中点
                        POINT endPoint(rowYendStraightside, 319);                            // 补线：终点

                        vector<POINT> input = {startPoint, midPoint, endPoint};
                        vector<POINT> b_modify = Bezier(0.01, input);
                        track.checkPointsEdgeRight.resize(rowRepairRingside);
                        track.checkPointsEdgeLeft.resize(rowRepairStraightside);
                        for (int kk = 0; kk < b_modify.size(); ++kk)
                        {
                            track.checkPointsEdgeLeft.emplace_back(b_modify[kk]);
                        }
                        break;
                    }
                }
            }
        }

        int tmp_ttttt = 0;
        if (ringEnable == false && ringStep == RingStep::Entering)
        {
            // 本场没判出环，且没有分叉
            if (!track.spurroad.empty() && rowRepairLine < track.checkPointsEdgeRight.size() - 1 && rowBreakpointRight > ROWSIMAGE / 2)
            {
                rowRepairStraightside = rowRepairLine;

                if (ringType == RingType::RingLeft)
                {
                    tmp_ttttt = 1;
                    for (int i = rowRepairLine; i < track.checkPointsEdgeLeft.size() - 1; i++)
                    {
                        if (track.checkPointsEdgeLeft[i].y <= 2 && i != track.widthBlock.size() - 1)
                        {
                            rowRepairRingside = i;
                            break;
                            // rowYendStraightside = track.checkPointsEdgeLeft[i].x;
                        }
                    }

                    for (int i = rowRepairRingside; i < track.checkPointsEdgeLeft.size() - 1; i++)
                    {
                        if (track.checkPointsEdgeLeft[i].y <= 2 && i != track.widthBlock.size() - 1)
                        {
                            rowYendStraightside = track.checkPointsEdgeLeft[i].x;
                        }
                        else if (rowRepairRingside != track.widthBlock.size() - 1)
                        {
                            int x = track.checkPointsEdgeLeft[rowRepairStraightside].x + (rowYendStraightside - track.checkPointsEdgeRight[rowRepairStraightside].x) * 5 / 4;
                            int y = (track.checkPointsEdgeLeft[rowRepairStraightside].y + track.checkPointsEdgeRight[rowRepairStraightside].y) / 2;

                            POINT startPoint = track.checkPointsEdgeRight[rowRepairStraightside]; // 补线：起点
                            POINT midPoint(x, y);                                                 // 补线：中点
                            POINT endPoint(rowYendStraightside, 0);                               // 补线：终点

                            // for (int i = 0; i < track.spurroad.size(); i++)
                            // {
                            //     if (track.spurroad[i].y < startPoint.y && track.spurroad[i].x < startPoint.x)
                            //         endPoint = track.spurroad[i];
                            //     break;
                            // }

                            vector<POINT> input = {startPoint, midPoint, endPoint};
                            vector<POINT> b_modify = Bezier(0.02, input);
                            track.checkPointsEdgeLeft.resize(rowRepairRingside);
                            track.checkPointsEdgeRight.resize(rowRepairStraightside);

                            for (int kk = 0; kk < b_modify.size(); ++kk)
                            {
                                track.checkPointsEdgeRight.emplace_back(b_modify[kk]);
                            }
                            break;
                        }
                    }
                }
                if (ringType == RingType::RingRight)
                {
                    tmp_ttttt = 1;
                    for (int i = rowRepairLine; i < track.checkPointsEdgeRight.size() - 1; i++)
                    {
                        if (track.checkPointsEdgeRight[i].y >= 318 && i != track.widthBlock.size() - 1)
                        {
                            rowRepairRingside = i;
                            break;
                            // rowYendStraightside = track.checkPointsEdgeLeft[i].x;
                        }
                    }

                    for (int i = rowRepairRingside; i < track.checkPointsEdgeRight.size() - 1; i++)
                    {
                        if (track.checkPointsEdgeRight[i].y >= 318 && i != track.widthBlock.size() - 1)
                        {
                            rowYendStraightside = track.checkPointsEdgeRight[i].x;
                        }
                        else if (rowRepairRingside != track.widthBlock.size() - 1)
                        {
                            int x = track.checkPointsEdgeRight[rowRepairStraightside].x + (rowYendStraightside - track.checkPointsEdgeLeft[rowRepairStraightside].x) * 5 / 4;
                            int y = (track.checkPointsEdgeRight[rowRepairStraightside].y + track.checkPointsEdgeLeft[rowRepairStraightside].y) / 2;

                            POINT startPoint = track.checkPointsEdgeLeft[rowRepairStraightside]; // 补线：起点
                            POINT midPoint(x, y);                                                 // 补线：中点
                            POINT endPoint(rowYendStraightside, 318);                               // 补线：终点

                            // for (int i = 0; i < track.spurroad.size(); i++)
                            // {
                            //     if (track.spurroad[i].y < startPoint.y && track.spurroad[i].x < startPoint.x)
                            //         endPoint = track.spurroad[i];
                            //     break;
                            // }

                            vector<POINT> input = {startPoint, midPoint, endPoint};
                            vector<POINT> b_modify = Bezier(0.02, input);
                            track.checkPointsEdgeRight.resize(rowRepairRingside);
                            track.checkPointsEdgeLeft.resize(rowRepairStraightside);

                            for (int kk = 0; kk < b_modify.size(); ++kk)
                            {
                                track.checkPointsEdgeLeft.emplace_back(b_modify[kk]);
                            }
                            break;
                        }
                    }
                }
            }
            // 本场没判出环，有分叉
            else
            {
                if (ringType == RingType::RingLeft && track.checkPointsEdgeRight.size() > 1)
                {
                    tmp_ttttt = 2;
                    int x_end = track.checkPointsEdgeRight[track.checkPointsEdgeRight.size() - 1].x;
                    for (int kkk = track.checkPointsEdgeRight[track.checkPointsEdgeRight.size() - 1].x; kkk < track.checkPointsEdgeRight[track.checkPointsEdgeRight.size() - 1].x + 50; kkk++)
                    {
                        if (imagePath.at<Vec3b>(kkk, 0)[2] > 0)
                        {
                            x_end = kkk;
                            break;
                        }
                    }

                    POINT startPoint(ROWSIMAGE - 10, COLSIMAGE - 1); // 补线：起点
                    POINT endPoint(x_end, 0);                        // 补线：终点

                    // for (int i = 0; i < track.spurroad.size(); i++)
                    // {
                    //     if (track.spurroad[i].y < startPoint.y && track.spurroad[i].x < startPoint.x)
                    //         endPoint = track.spurroad[i];
                    //     break;
                    // }
                    POINT midPoint = POINT((startPoint.x + endPoint.x) * 0.5, (startPoint.y + endPoint.y) * 0.5); // 补线：中点
                    vector<POINT> input = {startPoint, midPoint, endPoint};
                    vector<POINT> b_modify = Bezier(0.02, input);
                    track.checkPointsEdgeRight.resize(0);
                    track.checkPointsEdgeLeft.resize(0);
                    for (int kk = 0; kk < b_modify.size(); ++kk)
                    {
                        track.checkPointsEdgeRight.emplace_back(b_modify[kk]);
                    }
                }
                if (ringType == RingType::RingRight && track.checkPointsEdgeLeft.size() > 1)
                {
                    tmp_ttttt = 2;
                    int x_end = track.checkPointsEdgeLeft[track.checkPointsEdgeLeft.size() - 1].x;
                    for (int kkk = track.checkPointsEdgeLeft[track.checkPointsEdgeLeft.size() - 1].x; kkk < track.checkPointsEdgeLeft[track.checkPointsEdgeLeft.size() - 1].x + 50; kkk++)
                    {
                        if (imagePath.at<Vec3b>(kkk, 0)[2] > 0)
                        {
                            x_end = kkk;
                            break;
                        }
                    }

                    POINT startPoint(ROWSIMAGE - 10, 0);  // 补线：起点
                    POINT endPoint(x_end, COLSIMAGE - 1); // 补线：终点

                    // for (int i = 0; i < track.spurroad.size(); i++)
                    // {
                    //     if (track.spurroad[i].y < startPoint.y && track.spurroad[i].x < startPoint.x)
                    //         endPoint = track.spurroad[i];
                    //     break;
                    // }
                    POINT midPoint = POINT((startPoint.x + endPoint.x) * 0.5, (startPoint.y + endPoint.y) * 0.5); // 补线：中点
                    vector<POINT> input = {startPoint, midPoint, endPoint};
                    vector<POINT> b_modify = Bezier(0.02, input);
                    track.checkPointsEdgeRight.resize(0);
                    track.checkPointsEdgeLeft.resize(0);
                    for (int kk = 0; kk < b_modify.size(); ++kk)
                    {
                        track.checkPointsEdgeLeft.emplace_back(b_modify[kk]);
                    }
                }
            }
        }
        // 环中
        if (ringStep == RingStep::Entering && track.spurroad.empty() && counterSpurroad >= 3)
        {
            ringStep = RingStep::Inside;
        }
        // 出环补线
        if (ringStep == RingStep::Inside)
        {
            if (ringType == RingType::RingLeft)
            {
                int rowBreakRight = 0; // 右边缘横坐标连续性(行号)
                for (int i = 0; i < track.checkPointsEdgeRight.size(); i += 3)
                {
                    if (track.checkPointsEdgeRight[i].y <= track.checkPointsEdgeRight[rowBreakRight].y)
                    {
                        rowBreakRight = i;
                        continue;
                    }
                    if (i > rowBreakRight && track.checkPointsEdgeRight[i].y - track.checkPointsEdgeRight[rowBreakRight].y > 5)
                    {
                        rowBreakpointRight = rowBreakRight;
                        break; // 寻找到出环口：出环补线
                    }
                }
                track.checkPointsEdgeLeft.resize(0); // 单边控制
                int acute_angle_flag = 0;
                if (!track.checkPointsEdgeRight.empty() && track.checkPointsEdgeRight[rowBreakRight].y < COLSIMAGE / 4)
                {
                    track.checkPointsEdgeRight.resize(rowBreakRight); // 前80列不需要补线
                }
                else if (track.checkPointsEdgeRight.size() - rowBreakRight > 10)
                {
                    float slopeTop = 0;    // 斜率：分歧点上半部分
                    float slopeButtom = 0; // 斜率：分歧点下半部分
                    if (track.checkPointsEdgeRight[rowBreakRight].x != track.checkPointsEdgeRight[0].x)
                    {
                        slopeButtom = (track.checkPointsEdgeRight[rowBreakRight].y - track.checkPointsEdgeRight[0].y) * 100 /
                                      (track.checkPointsEdgeRight[rowBreakRight].x - track.checkPointsEdgeRight[0].x);
                    }
                    if (track.checkPointsEdgeRight[rowBreakRight].x != track.checkPointsEdgeRight[rowBreakRight + 20].x)
                    {
                        slopeTop = (track.checkPointsEdgeRight[rowBreakRight + 20].y - track.checkPointsEdgeRight[rowBreakRight].y) *
                                   100 / (track.checkPointsEdgeRight[rowBreakRight + 20].x - track.checkPointsEdgeRight[rowBreakRight].x);
                    }

                    if (slopeButtom * slopeTop <= 0)
                    {
                        rowBreakpointLeft = track.checkPointsEdgeRight[track.validRowsLeft].x;
                        POINT p_end(rowBreakpointLeft, 0); // 补线终点为左边有效行顶点
                        POINT p_mid((track.checkPointsEdgeRight[rowBreakRight].x + rowBreakpointLeft) * 3 / 8, track.checkPointsEdgeRight[rowBreakRight].y / 2);
                        vector<POINT> input = {track.checkPointsEdgeRight[rowBreakRight], p_mid, p_end};
                        vector<POINT> b_modify = Bezier(0.01, input);
                        //临时注销
                        track.checkPointsEdgeRight.resize(rowBreakRight);
                        for (int kk = 0; kk < b_modify.size(); ++kk)
                        {
                            track.checkPointsEdgeRight.emplace_back(b_modify[kk]);
                        }
                    }
                }
                else if (track.checkPointsEdgeRight.size() - rowBreakRight <= 10)
                {
                    _index = 2;
                    POINT p_end(rowBreakpointLeft, 0);
                    POINT p_start(max(rowBreakpointRight, ROWSIMAGE - 80), COLSIMAGE);
                    POINT p_mid((ROWSIMAGE - 50 + rowBreakpointLeft) / 4, COLSIMAGE / 2);
                    vector<POINT> input = {p_start, p_mid, p_end};
                    vector<POINT> b_modify = Bezier(0.01, input);
                    track.checkPointsEdgeRight.resize(0);
                    for (int kk = 0; kk < b_modify.size(); ++kk)
                    {
                        track.checkPointsEdgeRight.emplace_back(b_modify[kk]);
                    }
                }
            }
            if (ringType == RingType::RingRight)
            {
                int rowBreakLeft = 0; // 左边缘横坐标连续性(行号)
                for (int i = 0; i < track.checkPointsEdgeLeft.size(); i += 3)
                {
                    if (track.checkPointsEdgeLeft[i].y >= track.checkPointsEdgeLeft[rowBreakLeft].y)
                    {
                        rowBreakLeft = i;
                        // 待考量
                        continue;
                    }
                    // 待考量
                    if (i > rowBreakLeft && track.checkPointsEdgeLeft[i].y - track.checkPointsEdgeLeft[rowBreakLeft].y > 5)
                    {
                        rowBreakpointLeft = rowBreakLeft;
                        break; // 寻找到出环口：出环补线
                    }
                }
                track.checkPointsEdgeRight.resize(0); // 单边控制
                int acute_angle_flag = 0;
                if (!track.checkPointsEdgeLeft.empty() && track.checkPointsEdgeLeft[rowBreakLeft].y > (COLSIMAGE * 3) / 4)
                {
                    track.checkPointsEdgeLeft.resize(rowBreakLeft); // 前80列不需要补线
                }
                else if (track.checkPointsEdgeLeft.size() - rowBreakLeft > 10)
                {
                    float slopeTop = 0;    // 斜率：分歧点上半部分
                    float slopeButtom = 0; // 斜率：分歧点下半部分
                    if (track.checkPointsEdgeLeft[rowBreakLeft].x != track.checkPointsEdgeLeft[0].x)
                    {
                        // 待考量
                        slopeButtom = (track.checkPointsEdgeLeft[rowBreakLeft].y - track.checkPointsEdgeLeft[0].y) * 100 /
                                      (track.checkPointsEdgeLeft[rowBreakLeft].x - track.checkPointsEdgeLeft[0].x);
                    }
                    if (track.checkPointsEdgeLeft[rowBreakLeft].x != track.checkPointsEdgeLeft[rowBreakLeft + 20].x)
                    {
                        // 待考量
                        slopeTop = (track.checkPointsEdgeLeft[rowBreakLeft + 20].y - track.checkPointsEdgeLeft[rowBreakLeft].y) *
                                   100 / (track.checkPointsEdgeLeft[rowBreakLeft + 20].x - track.checkPointsEdgeLeft[rowBreakLeft].x);
                    }

                    if (slopeButtom * slopeTop <= 0)
                    {
                        rowBreakpointRight = track.checkPointsEdgeLeft[track.validRowsRight].x;
                        POINT p_end(rowBreakpointRight, 319); // 补线终点为左边有效行顶点
                        POINT p_mid((track.checkPointsEdgeLeft[rowBreakLeft].x + rowBreakpointRight) * 3 / 8, track.checkPointsEdgeLeft[rowBreakLeft].y / 2);
                        vector<POINT> input = {track.checkPointsEdgeLeft[rowBreakLeft], p_mid, p_end};
                        vector<POINT> b_modify = Bezier(0.01, input);
                        track.checkPointsEdgeLeft.resize(rowBreakLeft);
                        for (int kk = 0; kk < b_modify.size(); ++kk)
                        {
                            track.checkPointsEdgeLeft.emplace_back(b_modify[kk]);
                        }
                    }
                }
                else if (track.checkPointsEdgeLeft.size() - rowBreakLeft <= 10)
                {
                    _index = 2;
                    POINT p_end(rowBreakpointRight, 319);
                    POINT p_start(max(rowBreakpointLeft, ROWSIMAGE - 160), 0);
                    POINT p_mid((ROWSIMAGE - 190 - rowBreakpointRight) / 4, COLSIMAGE / 2);
                    vector<POINT> input = {p_start, p_mid, p_end};
                    vector<POINT> b_modify = Bezier(0.01, input);
                    track.checkPointsEdgeLeft.resize(0);
                    for (int kk = 0; kk < b_modify.size(); ++kk)
                    {
                        track.checkPointsEdgeLeft.emplace_back(b_modify[kk]);
                    }
                }
            }

            // 当左右边缘线点集小于图像的长度的三分之一判断出环岛
            if (max(rowBreakpointLeft, rowBreakpointRight) < ROWSIMAGE / 3)
            {
                ringStep = RingStep::Exiting;
            }
        }
        // 出环完成
        else if (ringStep == RingStep::Exiting)
        {
            if (ringType == RingType::RingLeft && rowBreakpointLeft < ROWSIMAGE / 2)
            {
                POINT p_end(rowBreakpointLeft, 0);
                POINT p_start(ROWSIMAGE - 50, COLSIMAGE - 1);
                POINT p_mid((ROWSIMAGE - 50 + rowBreakpointLeft) * 3 / 8, COLSIMAGE / 2);
                vector<POINT> input = {p_start, p_mid, p_end};
                vector<POINT> b_modify = Bezier(0.01, input);
                track.checkPointsEdgeRight.resize(0);
                track.checkPointsEdgeLeft.resize(0);
                for (int kk = 0; kk < b_modify.size(); ++kk)
                {
                    track.checkPointsEdgeRight.emplace_back(b_modify[kk]);
                }
                if (rowBreakpointRight > ROWSIMAGE / 2)
                {
                    ringStep = RingStep::Finish;
                }
            }
            if (ringType == RingType::RingRight && rowBreakpointRight < ROWSIMAGE / 2)
            {
                POINT p_end(rowBreakpointRight, 319);
                POINT p_start(ROWSIMAGE - 190, COLSIMAGE - 319);
                // 待考量
                POINT p_mid((ROWSIMAGE - 190 - rowBreakpointLeft) * 3 / 8, COLSIMAGE / 2);
                vector<POINT> input = {p_start, p_mid, p_end};
                vector<POINT> b_modify = Bezier(0.01, input);
                track.checkPointsEdgeRight.resize(0);
                track.checkPointsEdgeLeft.resize(0);
                for (int kk = 0; kk < b_modify.size(); ++kk)
                {
                    track.checkPointsEdgeLeft.emplace_back(b_modify[kk]);
                }
                if (rowBreakpointLeft > ROWSIMAGE / 2)
                {
                    ringStep = RingStep::Finish;
                }
            }
        }

        // //清掉边界的edge点
        // vector<POINT> v_temp, v_temp2;
        // for (int jj = 0; jj < track.checkPointsEdgeLeft.size(); ++jj)
        // {
        //     if (track.checkPointsEdgeLeft[jj].y > 2)
        //     {
        //         v_temp.push_back(track.checkPointsEdgeLeft[jj]);
        //     }
        //     else
        //     {
        //         if (jj > track.checkPointsEdgeLeft.size() * 9 / 10)
        //         {
        //             break;
        //         }
        //     }

        //     if (track.checkPointsEdgeLeft[jj].y > COLSIMAGE * 9 / 10 && jj < track.checkPointsEdgeLeft.size() - 5)
        //     {
        //         break;
        //     }
        // }
        // track.checkPointsEdgeLeft = v_temp;
        // if (track.checkPointsEdgeLeft.size() < 5)
        // {
        //     track.checkPointsEdgeLeft.resize(0);
        // }

        // for (int jj = 0; jj < track.checkPointsEdgeRight.size(); ++jj)
        // {
        //     if (track.checkPointsEdgeRight[jj].y < COLSIMAGE - 3)
        //     {
        //         v_temp2.push_back(track.checkPointsEdgeRight[jj]);
        //     }
        //     else
        //     {
        //         if (jj > track.checkPointsEdgeRight.size() * 9 / 10)
        //         {
        //             break;
        //         }
        //     }
        //     if (track.checkPointsEdgeRight[jj].y < COLSIMAGE / 10 && jj < track.checkPointsEdgeRight.size() - 5)
        //     {
        //         break;
        //     }
        // }
        // track.checkPointsEdgeRight = v_temp2;
        // if (track.checkPointsEdgeRight.size() < 5)
        // {
        //     track.checkPointsEdgeRight.resize(0);
        // }

        // 出环，切回正常循迹
        if (ringStep == RingStep::Finish)
        {
            if (track.checkPointsEdgeLeft.size() > 30 && track.checkPointsEdgeRight.size() > 30 &&
                (abs(track.checkPointsEdgeRight.size() - track.checkPointsEdgeLeft.size() < track.checkPointsEdgeRight.size() / 3) || abs(track.checkPointsEdgeLeft.size() - track.checkPointsEdgeRight.size() < track.checkPointsEdgeLeft.size() / 3)) &&
                track.spurroad.empty())
            {
                ringStep = RingStep::None;
            }
        }

        if (track.spurroad.empty())
            counterSpurroad++;
        else
            counterSpurroad = 0;

        //--------------------------------临时测试----------------------------------
        _ringStep = ringStep;
        _ringEnable = ringEnable;
        _tmp_ttttt = tmp_ttttt;

        // 返回识别结果
        if (ringStep == RingStep::None)
            return false;
        else
        {
            track.pointsEdgeLeft = track.checkPointsEdgeLeft;
            track.pointsEdgeRight = track.checkPointsEdgeRight;
            return true;
        }
    }

    /**
     * @brief 绘制环岛识别图像
     *
     * @param ringImage 需要叠加显示的图像
     */
    void drawImage(Tracking track, Mat &ringImage)
    {
        for (int i = 0; i < track.checkPointsEdgeLeft.size(); i++)
        {
            circle(ringImage, Point(track.checkPointsEdgeLeft[i].y, track.checkPointsEdgeLeft[i].x), 2,
                   Scalar(0, 255, 0), -1); // 绿色点
        }
        for (int i = 0; i < track.checkPointsEdgeRight.size(); i++)
        {
            circle(ringImage, Point(track.checkPointsEdgeRight[i].y, track.checkPointsEdgeRight[i].x), 2,
                   Scalar(0, 255, 255), -1); // 黄色点
        }

        for (int i = 0; i < track.spurroad.size(); i++)
        {
            circle(ringImage, Point(track.spurroad[i].y, track.spurroad[i].x), 5,
                   Scalar(0, 0, 255), -1); // 红色点
        }

        putText(ringImage, to_string(_ringStep) + " " + to_string(_ringEnable) + " " + to_string(_tmp_ttttt),
                Point(COLSIMAGE - 80, ROWSIMAGE - 20), cv::FONT_HERSHEY_TRIPLEX, 0.3, cv::Scalar(0, 0, 255), 1, CV_AA);

        putText(ringImage, to_string(_index), Point(80, ROWSIMAGE - 20), cv::FONT_HERSHEY_TRIPLEX, 0.3, cv::Scalar(0, 0, 255), 1, CV_AA);

        putText(ringImage, to_string(track.validRowsRight) + " | " + to_string(track.stdevRight),
                Point(COLSIMAGE - 100, ROWSIMAGE - 50),
                FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
        putText(ringImage, to_string(track.validRowsLeft) + " | " + to_string(track.stdevLeft),
                Point(30, ROWSIMAGE - 50), FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);

        putText(ringImage, "[7] RING - ENABLE", Point(COLSIMAGE / 2 - 30, 10), cv::FONT_HERSHEY_TRIPLEX, 0.3, cv::Scalar(0, 255, 0), 1, CV_AA);
        circle(ringImage, Point(_ringPoint.y, _ringPoint.x), 4, Scalar(255, 0, 0), -1); // 红色点
    }

private:
    uint16_t counterSpurroad = 0; // 岔路计数器
    // 临时测试用参数
    int _ringStep;
    int _ringEnable;
    int _tmp_ttttt;
    int _index = 0;
    POINT _ringPoint = POINT(0, 0);

    /**
     * @brief 环岛类型
     *
     */
    enum RingType
    {
        RingNone = 0, // 未知类型
        RingLeft,     // 左入环岛
        RingRight     // 右入环岛
    };

    /**
     * @brief 环岛运行步骤/阶段
     *
     */
    enum RingStep
    {
        None = 0, // 未知类型
        Entering, // 入环
        Inside,   // 环中
        Exiting,  // 出环
        Finish    // 环任务结束
    };

    RingType ringType = RingType::RingLeft; // 环岛类型
    RingStep ringStep = RingStep::None;     // 环岛处理阶段
    int rowRepairLine = 0;                  // 用于环补线的点（行号）
    int colRepairLine = 0;                  // 用于环补线的点（列号）
};
